#include <fstream>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <set>
#include <stdexcept>

#include "ply_reader.hh"

using namespace std;

// Convert a polygon index list into a list of triangles (here a
// triangle is just a list of indices).
void to_triangles(const vector<unsigned> poly,
                  vector<vector<unsigned> >& triangles)
{
  if (poly.size() < 3)
    {
      throw std::runtime_error("to_triangles error\n");
    }

  triangles.clear();
  triangles.resize(poly.size() - 2);

  if (poly.size() == 3)
    {
      vector<unsigned>& tri = triangles[0];
      tri.push_back(poly[0]);
      tri.push_back(poly[1]);
      tri.push_back(poly[2]);
      return;
    }
  
  // i1 stays at the first vertex throughout the algorithm.
  unsigned i1 = poly[0];
  unsigned i2 = poly[1];
  for (unsigned i = 2, k = 0; i < poly.size(); ++i, ++k)
    {
      unsigned i3 = poly[i];
      vector<unsigned>& tri = triangles[k];
      tri.push_back(i1);
      tri.push_back(i2);
      tri.push_back(i3);

      i2 = i3;
    }
}


void read_ply (const char* file,
               std::vector<cgmath::triangle_3d>& triangles)
{
  ply_reader reader;
  vector<float> vertex_data;
  vector<unsigned> index_data;
  reader.readMesh(file, vertex_data, index_data);

  triangles.clear();
  // Each face in the index_data list consists of three indices. The
  // three indices for each face is stored consecutively in the list.
  // All in all there are thus index_data.size() / 3 faces.
  unsigned n = index_data.size() / 3;
  for (unsigned i = 0; i < n; ++i)
    {
      cgmath::vector_n<float, 3> v0;
      cgmath::vector_n<float, 3> v1;
      cgmath::vector_n<float, 3> v2;

      unsigned offset;

      offset = index_data[i * 3 + 0];
      v0(0) = vertex_data[offset * 3 + 0];
      v0(1) = vertex_data[offset * 3 + 1];
      v0(2) = vertex_data[offset * 3 + 2];

      offset = index_data[i * 3 + 1];
      v1(0) = vertex_data[offset * 3 + 0];
      v1(1) = vertex_data[offset * 3 + 1];
      v1(2) = vertex_data[offset * 3 + 2];

      offset = index_data[i * 3 + 2];
      v2(0) = vertex_data[offset * 3 + 0];
      v2(1) = vertex_data[offset * 3 + 1];
      v2(2) = vertex_data[offset * 3 + 2];

      triangles.push_back(cgmath::triangle<float, 3>(v0, v1, v2));
    }
}

int ply_reader::readMesh(string plyFile,
                        std::vector<float> &vertexData,
                        std::vector<unsigned> &indexData)
{
  readply(plyFile, vertexData, indexData);
  return 1;
}

void ply_reader::splitString(string s, vector<string> &strings)
{
  unsigned begin = 0;
  //unsigned k = 0;
  unsigned end;

  for (end = 0; end < s.size(); ++end) {
    if (s[end] == ' ' || s[end] == '\t') {
      strings.push_back(s.substr(begin, end - begin));
      begin = end + 1;
    }

    for (; (end < s.size()) && (s[end] == ' ' || s[end] == '\t') ; ++end) {
    }
  }
  
  if (end - begin > 0) {
    strings.push_back(s.substr(begin, end - begin));
  }
}

int ply_reader::readply(const string fileName,
                        std::vector<float> &vertexData,
                        std::vector<unsigned> &indexData)
{
  int bufsize = 1000;
  // float confidence;
  // float intensity;
  int nVertices = 0;
  int nFaces = 0;

  char *buf = new char[bufsize];
  ifstream ifs(fileName.c_str(), ios_base::in);

  string end_header("end_header");
  
  do {
    ifs.getline(buf, bufsize);
    string line(buf);
    vector<string> strings;
    splitString(line, strings);

    if (strings.size() == 3) {
      if (strings[0].compare("element") == 0 &&
          strings[1].compare("vertex") == 0) {
        nVertices = atoi(strings[2].c_str());
      }

      if (strings[0].compare("element") == 0 &&
          strings[1].compare("face") == 0) {
        nFaces = atoi(strings[2].c_str());
      }
    }
  } while (end_header.compare(buf) != 0);
  
  // Start reading vertex data.
  for (int i = 0; i < nVertices; ++i) {
    vector<string> strings;
    ifs.getline(buf, bufsize);
    string line(buf);
    splitString(line, strings);
    
    float x1 = (float)atof(strings[0].c_str());
    float x2 = (float)atof(strings[1].c_str());
    float x3 = (float)atof(strings[2].c_str());

    vertexData.push_back(x1);
    vertexData.push_back(x2);
    vertexData.push_back(x3);
  }

  // We use faceSet to handle duplicate faces.
  std::set<OrderedTriplet> faceSet;
  for (int i = 0; i < nFaces; ++i) {
    vector<string> strings;
    ifs.getline(buf, bufsize);
    string line(buf);
    splitString(line, strings);

    vector<unsigned> poly;
    unsigned count = atoi(strings[0].c_str());
    for (unsigned j = 1; j <= count; ++j)
      {
        poly.push_back(atoi(strings[j].c_str()));
      }

    vector<vector<unsigned> > triangles;
    to_triangles(poly, triangles);
    
    for (unsigned j = 0; j < triangles.size(); ++j)
      {
        vector<unsigned>& tri = triangles[j];
        OrderedTriplet trip(tri[0], tri[1], tri[2]);
        if (faceSet.find(trip) == faceSet.end()) {
          faceSet.insert(trip);
          indexData.push_back(tri[0]);
          indexData.push_back(tri[1]);
          indexData.push_back(tri[2]);
        } 
      }
  }
  
  delete [] buf;

  return 0;
  
}
